// RIT Robotic Systems 2011-3 Project: Blue Steel
// Matt Hodge and Max Macri
//

#include <Ping.h> 
#include <Movement.h>

 // Constants
 const int stop_inches = 9; // inches before an object to stop
 const int min_forwardTime = 2000; // ms
 const int max_forwardTime = 4001; // ms (+mintime-1)
 const int min_turnTime = 500; // ms
 const int max_turnTime = 1501; // ms (+mintime-1)
 const int max_irThreshold = 600; // 0-1024
 const int difference_irThreshold = 100; 
 // Pins
 int leftIRPin = A0;
 int rightIRPin = A3;
 
 // Objects
 Ping ping = Ping(5,0,29); //Pin,in_offset,cm_offset //74
 Movement move = Movement(6,7,8,9,11,10);
 
 // Primatives
 int leftIRVal = 0;
 int rightIRVal = 0;
 long randMove = 0;
 long randTurn = 0;
 int hort_move = 0;

 bool seeIR = false;
 
void setup() {
  // Begin Debug
  Serial.begin(9600);
  // Seed PRNG
  randomSeed(analogRead(0));
}

void loop() {
  int moveTime = 0;
  
  // move forward for a random time (2000-6000ms)
  randMove = random(max_forwardTime);
  randMove += min_forwardTime;
  
  // move forward for the generated time, avoiding objects, then turn
  while(moveTime < randMove){
    // check for objects
    ping.fire();
    Serial.print("Inches: ");
    Serial.print(ping.inches());
    Serial.print("\t");
    
    // check IR
    leftIRVal = analogRead(leftIRPin);
    rightIRVal = analogRead(rightIRPin);
    
    if(ping.inches() <= stop_inches){
      // Object within range, turn to aviod
      Serial.println("RotateRight(PING)");
      move.setMove(25,50);
      moveTime = 0;
      
    }else if( leftIRVal <= max_irThreshold && rightIRVal <= max_irThreshold){ 
      // IR source has been spotted ahead, move towards it
      
      
      //if(abs(rightIRVal-leftIRVal) <= difference_irThreshold){
      if(leftIRVal <= 350 && rightIRVal <=350){
        move.setMove(50,25);
        Serial.println("IRFORWARD(50,25)(DIFFTOL)");
      }else{
        hort_move = (rightIRVal-leftIRVal+max_irThreshold)/(max_irThreshold/50)+25;
        move.setMove(hort_move,25);
        delay(10);
        move.setMove(hort_move,25);
        Serial.print("IRFORWARD(");
        Serial.print(hort_move);
        Serial.print(",25)(DIFFTOL)\n");
      }      
      moveTime = 0;
        
      
    }else if( leftIRVal <= max_irThreshold){ 
      // IR source has been spotted left, turn left towards it
      Serial.println("IRTurnLeft(75,50)");
      move.setMove(75,50);
      moveTime = 0;
      
    }else if( rightIRVal <= max_irThreshold){ 
      // IR source has been spotted right, turn right towards it
      Serial.println("IRTurnRight(25,50)");
      move.setMove(25,50);
      moveTime = 0;
      
    } else {
      // Move forward for random time
      Serial.print("Forward: ");
      Serial.print(moveTime);
      Serial.print(":");
      Serial.print(randMove);
      Serial.print("\n");
      
      move.setMove(50,25);
      moveTime += 50;
    }
    
    delay(50);
  }
  Serial.println("RotateRight(RANDOM)");
  randTurn = random(max_turnTime);
  randTurn += min_turnTime;
  
  //moveTime = 0;
  //while(moveTime <= randTurn){
    //move.setMove(100,50);
    //moveTime += 250;
    //delay(250);
  //}
  
  // (need to call setMove twice)
  move.setMove(25,50);
  delay(50);
  move.setMove(25,50);
  delay(randTurn);
  
}
